#ifdef _WIN32
#include <winsock2.h>
#else
#include <sys/types.h> 
#include <sys/socket.h>
#include <stdio.h>
#include <netinet/in.h>
#include <arpa/inet.h> 
#include <sys/un.h>
#define closesocket(param) close(param)
#endif
#include "opencv/cvaux.h"
#include "opencv/highgui.h"
#include "opencv/cxcore.h"
#include <pthread.h>
#include <stdio.h>
#include <time.h>
#include "jpeg_streamer.h"


#define JPEG_QUALITY 20


#define STATE_HISTOGRAMM 0
#define STATE_DETECTION 1

int int_fps =  1 ;
time_t current_time;
time_t last_time ;
int nb_frame = 0 ;
int last_pos [2];
CvCapture* camera ;
CvMemStorage* storage ;
float range[] = { 0, 180 };
int bins [] = {10} ;
float* ranges[] = { range};
CvHistogram* ref_hist ; 
IplImage* ping_pong [2] ;
int ping_pong_read_index = 0 ;
int ping_pong_write_index = 0 ;
pthread_mutex_t ping_pong_mutexes [2] = {PTHREAD_MUTEX_INITIALIZER, PTHREAD_MUTEX_INITIALIZER};
pthread_mutex_t index_mutex = PTHREAD_MUTEX_INITIALIZER ;
int port = 9748 ;
int state = STATE_HISTOGRAMM;



CvSeq* detect_ball_hsv(IplImage * frame, CvMemStorage* storage);
CvSeq* detect_ball_gray(IplImage * frame, CvMemStorage* storage);
void filter_candidates(IplImage *  hsv_frame, CvSeq* circles);
void display_histogram(CvHistogram * hist, int num_bins, char* window);
int deriv_histogram(CvHistogram * hist, double* deriv_hist, int num_bins);
CvHistogram* create_reference_histogram(int nb_bins, float range_min, float range_max);
double compute_distance(int x1, int y1, int x2, int y2);
void init_vision();
void detect_ball(IplImage* frame);
void capture_ref_histrogramm(IplImage* new_frame);
void display_candidates(IplImage *  hsv_frame, IplImage *  frame ,CvSeq* circles);
void * vision(void * arg) ;
void *server(void * arg) ;






void * vision(void * arg)
{   
  int nb_ball = 0 ;
  char key = 'o';
#ifdef DISPLAY
  cvNamedWindow("result", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("ball_hist", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("intermediate", CV_WINDOW_AUTOSIZE);
#endif
  do{
    do {
      pthread_mutex_lock( &ping_pong_mutexes[ping_pong_write_index]);
      ping_pong[ping_pong_write_index] = cvQueryFrame(camera);
      capture_ref_histrogramm(ping_pong[ping_pong_write_index]);
      pthread_mutex_unlock( &ping_pong_mutexes[ping_pong_write_index]);
      pthread_mutex_lock(&index_mutex);
      ping_pong_read_index = ping_pong_write_index ;
      ping_pong_write_index = (ping_pong_write_index + 1) % 2 ;
      pthread_mutex_unlock(&index_mutex);
    } while(state == STATE_HISTOGRAMM);
    do {
      pthread_mutex_lock( &ping_pong_mutexes[ping_pong_write_index]);
      ping_pong[ping_pong_write_index] = cvQueryFrame(camera);
      detect_ball(ping_pong[ping_pong_write_index]);
      pthread_mutex_unlock( &ping_pong_mutexes[ping_pong_write_index]);
      pthread_mutex_lock(&index_mutex);
      ping_pong_read_index = ping_pong_write_index ;
      ping_pong_write_index = (ping_pong_write_index + 1) % 2 ;
      pthread_mutex_unlock(&index_mutex);
    } while(state == STATE_DETECTION);
  }while(state == STATE_DETECTION || state == STATE_HISTOGRAMM);
  cvReleaseCapture(&camera);
  return 0;
}



void capture_ref_histrogramm(IplImage* frame){
  IplImage *  hsv_frame    = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
  char key = 'q';
  IplImage* h_plane = cvCreateImage( cvGetSize(frame), 8, 1 );
  int x = cvRound((frame -> width /2)-(0.707*(frame -> height/4))) + 5;
  int y = cvRound((frame -> height /2 ) -(0.707*(frame -> height/4))) + 5 ;
  int height = cvRound((frame -> height/4)*1.41);
  int width = height;
  if(x < 0){
    width = width + x ;
    x = 0 ;
  }
  if(y < 0){
    height = height + y ;
    y = 0 ;
  }
  if((x + width) > frame -> width){
    width = frame -> width - x -10; 
  }
  if((y + height) > frame -> height){
    height = frame -> height - y -10 ;
  }
  if (frame != NULL) {
    cvResetImageROI(h_plane);
    cvCvtColor(frame, hsv_frame, CV_BGR2HSV);
    cvCvtPixToPlane( hsv_frame, h_plane, 0, 0, 0 );
    cvSetImageROI(h_plane, cvRect(x, y, width, height));
    cvCalcHist( &h_plane , ref_hist, 0, 0 );
    cvCircle( frame, cvPoint((frame -> width /2),(frame -> height /2)), frame -> height /4, CV_RGB(255,0,0), 3, 8, 0 );
    cvNormalizeHist(ref_hist, 1.0);
#ifdef DISPLAY
    cvShowImage("result",frame );
    display_histogram(ref_hist, bins[0], "ball_hist");
#endif

  }
  cvReleaseImage(&hsv_frame);
  cvReleaseImage(&h_plane);
}

void detect_ball(IplImage* frame){
  if (frame != NULL) {

    nb_frame ++ ;
    current_time = time(NULL) ;
    if(current_time - last_time >= 1){
      float fps =  ((float) nb_frame) / (current_time - last_time);
      if(fps < 1){
        int_fps = 1 ;
      }else{
        int_fps = cvRound(fps);
      }
      last_time = current_time ;
      nb_frame = 0 ;
      printf("%f FPS \n", fps);
    }
    CvSeq* circles = detect_ball_gray(frame, storage);
    //CvSeq* circles = detect_ball_hsv(frame, storage);
    double max_distance = frame -> width ;
    float * best_circle = NULL ;
    for (int i = 0; i < circles->total; i++)
    {
      float* p = (float*) cvGetSeqElem( circles, i );
      double distance = compute_distance(cvRound(p[0]), cvRound(p[1]), last_pos[0], last_pos[1]);
      if(distance < max_distance){
        best_circle = p ;
        max_distance = distance ;
      }
    }
    if(best_circle != NULL){
      last_pos[0] = cvRound(best_circle[0]); 
      last_pos[1] = cvRound(best_circle[1]); 
      cvCircle( frame, cvPoint(last_pos[0],last_pos[1]), 3, CV_RGB(0,255,0), -1, 8, 0 );
      cvCircle( frame, cvPoint(last_pos[0],last_pos[1]), cvRound(best_circle[2]), CV_RGB(255,0,0), 3, 8, 0 );
#ifdef SAVE_IMAGE 
      char file_name [50]; 
      sprintf(file_name, "%d.jpg", nb_ball);
      cvSaveImage(file_name, frame);
      nb_ball ++ ;
#endif
      //printf("ball detected at x:%d y:%d of radius %d \n", last_pos[0], last_pos[1], cvRound(best_circle[2]));
    }
#ifdef DISPLAY
    cvShowImage("result",frame );
    cvWaitKey(80);
#endif
  }


}


void init_vision(){
  camera = cvCreateCameraCapture(-1); // Use the default camera
  ping_pong[0] = cvQueryFrame(camera);
  ping_pong[1] = cvQueryFrame(camera);
  ping_pong[0] = cvQueryFrame(camera);
  last_pos[0] = 0 ;
  last_pos[1] = 0 ;
  storage = cvCreateMemStorage(0);
  ref_hist = cvCreateHist( 1, bins, CV_HIST_ARRAY, ranges );
  /*#ifdef DISPLAY
  cvNamedWindow("result", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("ball_hist", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("intermediate", CV_WINDOW_AUTOSIZE);
  #endif*/
  last_time = time(NULL);
}



CvSeq* detect_ball_hsv(IplImage * frame, CvMemStorage* storage){
  CvSize size = cvSize(frame -> width ,frame -> height);
  CvScalar hsv_min = cvScalar(0, 70, 70, 0);
  CvScalar hsv_max = cvScalar(50, 250, 256, 0);
  CvScalar hsv_min2 = cvScalar(200, 70, 70, 0);
  CvScalar hsv_max2 = cvScalar(256, 250, 256, 0);
  IplImage *  hsv_frame    = cvCreateImage(size, IPL_DEPTH_8U, 3);
  IplImage*  thresholded    = cvCreateImage(size, IPL_DEPTH_8U, 1);
  IplImage*  thresholded2    = cvCreateImage(size, IPL_DEPTH_8U, 1);
  cvCvtColor(frame, hsv_frame, CV_BGR2HSV);
  cvInRangeS(hsv_frame,hsv_min,hsv_max,thresholded);
  cvInRangeS(hsv_frame,hsv_min2,hsv_max2,thresholded2);
  cvOr(thresholded,thresholded2,thresholded);
  cvSmooth( thresholded, thresholded, CV_GAUSSIAN, 9, 9 );
#ifdef DISPLAY
  cvShowImage("intermediate",thresholded);
#endif
  CvSeq* circles = cvHoughCircles(thresholded, storage, CV_HOUGH_GRADIENT, 2, thresholded->height/2, 100, 40, (frame -> width)/8, (frame -> width)/4);
  filter_candidates(hsv_frame, circles);
  cvReleaseImage(&thresholded);
  cvReleaseImage(&thresholded2);
  cvReleaseImage(&hsv_frame);
  return circles ;
}


CvSeq* detect_ball_gray(IplImage * frame, CvMemStorage* storage){
  clock_t a = 0;
  double max_dev_value = 0 ;
  int max_index= 0;
  IplImage*  gray    = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 1);
  IplImage *  hsv_frame    = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
  cvCvtColor( frame, gray, CV_BGR2GRAY );
  cvCvtColor(frame, hsv_frame, CV_BGR2HSV);
#ifdef DISPLAY
  cvShowImage("intermediate",gray);
#endif
  cvSmooth( gray, gray, CV_GAUSSIAN, 9, 9 );
  CvSeq* circles = cvHoughCircles(gray, storage, CV_HOUGH_GRADIENT, 2,  frame->height/2, 100, 40, (frame -> width)/8, (frame -> width)/4);
  display_candidates(hsv_frame, frame, circles);
  cvReleaseImage(&gray);
  cvReleaseImage(&hsv_frame);
  return circles ;
}


void filter_candidates(IplImage *  hsv_frame, CvSeq* circles){
  double max_dev_value = 0 ;
  int max_index= 0 ;
  CvHistogram* hist = cvCreateHist( 1, bins, CV_HIST_ARRAY, ranges );
  IplImage* h_plane = cvCreateImage( cvGetSize(hsv_frame), 8, 1 );
  cvCvtPixToPlane( hsv_frame, h_plane, 0, 0, 0 );
  for (int i = 0; i < circles->total; )
  {
    float max_value = 0;
    int bin_index = 0;
    float* circle = (float*)cvGetSeqElem( circles, i );
    int x = cvRound(circle[0]-(0.707*circle[2])) ;
    int y = cvRound(circle[1]-(0.707*circle[2])) ;
    int height = cvRound(circle[2]*1.41);
    int width = height;
    if(x < 0){
      width = width + x ;
      x = 0 ;
    }
    if(y < 0){
      height = height + y ;
      y = 0 ;
    }
    if((x + width) > hsv_frame -> width){
      width = hsv_frame -> width - x;
    }
    if((y + height) > hsv_frame -> height){
      height = hsv_frame -> height - y;
    }
    cvSetImageROI(h_plane, cvRect(x, y, width, height));
    cvCalcHist( &h_plane , hist, 0, 0 );
    cvGetMinMaxHistValue( hist, 0, &max_value, 0, &bin_index );
    cvResetImageROI(h_plane);
    cvNormalizeHist(hist, 1.0);
    double corel = cvCompareHist(hist, ref_hist, CV_COMP_CORREL );
    if(corel < 0.8){
      cvSeqRemove(circles, i);
    }else{
      i ++ ;
    }
  }
  cvReleaseImage(&h_plane);
  cvReleaseHist(&hist);
}


void display_candidates(IplImage *  hsv_frame, IplImage *  frame ,CvSeq* circles){
  CvFont font;
  char text [20] ;
  cvInitFont( &font, CV_FONT_HERSHEY_SIMPLEX,1.0,1.0,0,8,16);
  double max_dev_value = 0 ;
  int max_index= 0 ;
  CvHistogram* hist = cvCreateHist( 1, bins, CV_HIST_ARRAY, ranges );
  IplImage* h_plane = cvCreateImage( cvGetSize(hsv_frame), 8, 1 );
  cvCvtPixToPlane( hsv_frame, h_plane, 0, 0, 0 );
  for (int i = 0; i < circles->total; i ++)
  {
    float max_value = 0;
    int bin_index = 0;
    float* circle = (float*)cvGetSeqElem( circles, i );
    int x = cvRound(circle[0]-(0.707*circle[2])) ;
    int y = cvRound(circle[1]-(0.707*circle[2])) ;
    int height = cvRound(circle[2]*1.41);
    int width = height;
    if(x < 0){
      width = width + x ;
      x = 0 ;
    }
    if(y < 0){
      height = height + y ;
      y = 0 ;
    }
    if((x + width) > frame -> width){
      width = frame -> width - x;
    }
    if((y + height) > frame -> height){
      height = frame -> height - y;
    }
    cvSetImageROI(h_plane, cvRect(x, y, width, height));
    cvCalcHist( &h_plane , hist, 0, 0 );
    cvGetMinMaxHistValue( hist, 0, &max_value, 0, &bin_index );
    cvResetImageROI(h_plane);
    cvNormalizeHist(hist, 1.0);
    double corel = cvCompareHist(hist, ref_hist, CV_COMP_CORREL );
    sprintf(text,"%f", corel);
    cvPutText(frame, text, cvPoint(cvRound(circle[0]),cvRound(circle[1])), &font, CV_RGB(0,255,0));
    cvCircle( frame, cvPoint(cvRound(circle[0]),cvRound(circle[1])), cvRound(circle[2]), CV_RGB(255,0,0), 3, 8, 0 );
  }
  cvReleaseImage(&h_plane);
  cvReleaseHist(&hist);
}


double compute_distance(int x1, int y1, int x2, int y2){
  return sqrt(pow(x1 - x2,2.0) + pow(y1 - y2, 2.0));
}


int deriv_histogram(CvHistogram * hist, double* deriv_hist, int num_bins){
  double max = 0;
  int max_bin = 0 ;
  for( int i = 0; i < (num_bins-1); i++ ) {
    deriv_hist[i] = cvGetReal1D(hist->bins,i) - cvGetReal1D(hist->bins,i+1);
    if(deriv_hist[i] > max){
      max = deriv_hist[i] ;
      max_bin = i ;
    }
  }

  return max_bin ;
}

void display_histogram(CvHistogram * hist, int num_bins, char* window){
  float max_value = 0 ;
  IplImage *  hist_image    = cvCreateImage(cvSize(num_bins*15,200), IPL_DEPTH_8U, 3);
  cvSet(hist_image, cvScalar(0,0,0,0));
  cvGetMinMaxHistValue( hist, 0, &max_value, 0, 0 );
  for( int i = 0; i < num_bins; i++ ) {
    CvPoint b = cvPoint((i*10), 10);
    CvPoint t = cvPoint((i*10)+10, (cvGetReal1D(hist->bins,i)/cvRound(max_value))*180);
    cvRectangle(hist_image, b, t, cvScalar(150,150,150,0), CV_FILLED);
  }
  cvFlip(hist_image,NULL,0);
  cvShowImage(window, hist_image);
  cvReleaseImage(&hist_image);
}


CvHistogram* create_reference_histogram(int nb_bins, float range_min, float range_max){
  float range[] = { range_min, range_max };
  int bins [] = {nb_bins} ;
  float* ranges[] = {range};
  CvHistogram* hist = cvCreateHist( 1, bins, CV_HIST_ARRAY, ranges );
  for(int i = 0 ; i < nb_bins ; i ++){
    if(i == 0 ){//|| i == (nb_bins - 1)){
      cvSetReal1D(hist->bins, i, 1.0);
    }else{
      cvSetReal1D(hist->bins, i, 0.0);
    }
  }
  return hist ;
}





int main(int argc, char ** argv)
{
  int rc1, rc2, i;
  bool with_server = true ;
  port = 9745 ;
  pthread_t thread1, thread2;
#ifdef _WIN32
  for(i = 1 ; i < argc ; i ++){
    if(argv[i] = "s"){
      with_server = true ;
    }else if(argv[i] = "p"){
      if(i < argc){
        port = atoi(argv[i+1]);
        i ++ ;
      }
    }
  }
#else
  while ((c = getopt (argc, argv, "vp:h:i:s:f:m:d:")) != EOF) {
    switch (c) {
      case 's':
        with_server = true ;
        break;
      case 'p':
        port = atoi (optarg);
        break;
      default:
        usage (argv[0]);
        break;
    }
  }
#endif

  /* Create independent threads each of which will execute functionC */
  init_vision();
  if( (rc1=pthread_create( &thread1, NULL,  &vision, NULL)) )
  {
    printf("Thread creation failed: %d\n", rc1);
  }
  if(with_server){
    if( (rc2=pthread_create( &thread2, NULL,  &server, NULL)) )
    {
      printf("Thread creation failed: %d\n", rc1);
    }
  }
  pthread_join( thread1, NULL);
  if(with_server) pthread_join( thread2, NULL);
  exit(0);
}

void *server(void * arg)
{
  char  filename [50];
  char recv_buffer [20];
  int nb_received = 0 ;
  unsigned char ** jpeg_buffer ;
  long unsigned int * buffer_length ; 
#ifdef _WIN32 
  WSADATA WSAData ;
  WSAStartup(MAKEWORD(2,0), &WSAData);
#endif

  SOCKET sockfd_serveur, sockfd_client ;
  SOCKET long_serveur ; 
  int long_client ;
  time_t a ;
  unsigned char * outbuffer [1] = {NULL};
  long unsigned int * outlen = (long unsigned int *) malloc(sizeof(long unsigned int));
  struct sockaddr_in adresse_serveur ;
  struct sockaddr_in adresse_client ;
  int bytes = 0 ;
  char end_frame [2]= {0xFF, 0xD9};

  sockfd_serveur = socket( AF_INET, SOCK_STREAM, 0) ;

  adresse_serveur.sin_family = AF_INET ;
  adresse_serveur.sin_addr.s_addr =  inet_addr("127.0.0.1") ;
  adresse_serveur.sin_port = htons(port) ;
  long_serveur = sizeof( adresse_serveur ) ;
  bind( sockfd_serveur, (struct sockaddr *)&adresse_serveur, long_serveur) ;
  listen(sockfd_serveur, 5) ;
  long_client = sizeof( adresse_client ) ;
  sockfd_client = accept(sockfd_serveur, (struct sockaddr *)&adresse_client, &long_client) ;
  printf("connected \n");
  while(1)
  {

    //sprintf(filename,"C:\\myfile%f.jpeg", time(NULL));
    //FILE * outfile = fopen( filename, "wb" );
    pthread_mutex_lock( &ping_pong_mutexes[ping_pong_read_index]);
    pthread_mutex_lock(&index_mutex);
    a = time(NULL);
    bytes = write_jpeg_iplImage(ping_pong[ping_pong_read_index], sockfd_client, JPEG_QUALITY);
    //bytes = write_jpegfile_iplImage(ping_pong[ping_pong_read_index], outfile, JPEG_QUALITY);
    //write_jpegmem_iplImage(ping_pong[ping_pong_read_index], outbuffer, outlen, JPEG_QUALITY);
    //bytes = send(sockfd_client, (char *) *outbuffer, *outlen, 0);
    bytes = send(sockfd_client, end_frame, 2, 0) ;
    //fwrite((void *) *outbuffer, sizeof(char), *outlen, outfile);
    //fwrite((void *) end_frame, sizeof(char), 2, outfile);
    a = time(NULL) - a ;
    printf("jpeg encoding : %d ms \n", (int) a);
    pthread_mutex_unlock( &ping_pong_mutexes[ping_pong_read_index]);
    pthread_mutex_unlock(&index_mutex);
    /*if((nb_received= recv(sockfd_client, recv_buffer, 20, NON_BLOCKING)) > 0){
      parse_command(recv_buffer, nb_received);
    }*/
    //fclose(outfile);
    if (bytes <= 0) 
    {
      fprintf(stderr, "Connection closed.\n");
      closesocket(sockfd_client);
      state = STATE_DETECTION ;
      while((sockfd_client = accept(sockfd_serveur, (struct sockaddr *)&adresse_client, &long_client)) == -1); // wait for new connection
    }
#ifdef _WIN32 
    Sleep(200);
#else
    usleep(200000);
#endif

  }
#ifdef WIN32
  WSACleanup();
#endif
  return NULL ;
}